#ifndef CV_MAT_HPP
#define CV_MAT_HPP

#include <assert.h>
#include <memory>

#include <qhcomplex.h>
#include <qhmath.h>
#include <qhdsp.h>
#include <qhblas.h>

#include <qhmath_hvx.h>
#include <qhdsp_hvx.h>
#include <qhdsp_hvx_common.h>
#include <qhblas_hvx.h>

#include "utils.hpp"

#include "qfe.h"

template<typename T>
struct CvPoint2
{
    CvPoint2() = default;
    CvPoint2(T x_, T y_):x(x_),y(y_){}
    T x;
    T y;
};

typedef CvPoint2<float> CvPoint2f;


template<typename T>
struct CvVec2
{
    CvVec2() = default;
    CvVec2(T x_, T y_):x(x_),y(y_){}
    T x;
    T y;
};

template<typename T>
struct CvRect_
{
    CvRect_() = default;
    CvRect_(T x_, T y_, T w, T h):x(x_),y(y_),width(w),height(h){}
    T x;
    T y;
    T width;
    T height;
};

typedef CvRect_<int32_t> CvRect;
typedef CvRect_<float> CvRectf;

template<typename T>
bool operator != (const CvRect_<T> &lh, const CvRect_<T> &rh) {
    return !(lh.x == rh.x &&
            lh.y == rh.y &&
            lh.width == rh.width &&
            lh.height == rh.height);
}

template <typename T, int alginN = 1024>
class CvMat
{
public:
    explicit CvMat():data(nullptr),rows(-1),cols(-1),stride(-1),mem_ptr(nullptr),ref_cnt(nullptr) {
    }
    CvMat(int rows_, int cols_, int stride_ = -1): CvMat() {
        resize(rows_, cols_, stride_); 
    }
    explicit CvMat(int size): CvMat(1, size) {}

    CvMat(const CvMat & h) {
        this->data = h.data;
        this->cols = h.cols;
        this->rows = h.rows;
        this->ref_cnt = h.ref_cnt;
        this->stride = h.stride;
        this->mem_ptr = h.mem_ptr;
        if (ref_cnt) {
            ref_cnt[0]++;
        }
    }
    CvMat(T*data_, int rows_, int cols_, int stride_ = -1): CvMat() {
        data = data_;
        rows = rows_;
        cols = cols_;
        stride = (stride_ == -1) ? cols_*sizeof(T) : stride_;
        mem_ptr = data;
        ref_cnt = nullptr; // this is null when does't own data, and we are not set data ref_cnt
    }

    CvMat& operator = (const CvMat &h) {
        if (this != &h) {
            if (ref_cnt && ref_cnt[0]) {
                ref_cnt[0]--;
                if (ref_cnt[0] == 0) {
                    // printf("---d--operator=  %p, %d\n", data, ref_cnt[0]);
                    if (mem_ptr)free(mem_ptr);
                }
            }
            this->data = h.data;
            this->ref_cnt = h.ref_cnt;
            this->rows = h.rows;
            this->cols = h.cols;
            this->stride = h.stride;
            this->mem_ptr = h.mem_ptr;
            if (ref_cnt) {
                ref_cnt[0]++;
            }
            // printf("-----operator= %p, %d\n", data, ref_cnt[0]);
        }
        return *this;
    }

    ~CvMat() {
        // printf("----- ~CvMat %p %p\n", data, ref_cnt);
        // check if own data and need to free data
        if(ref_cnt && ref_cnt[0]) {
            ref_cnt[0]--;
            if (ref_cnt[0] == 0) {
                // printf("----- ~CvMat delete %p\n", data);
                if (mem_ptr)free(mem_ptr);
            }
        }
    }
 
    void zeros() {
        // assert(data != nullptr);
        memset(data, 0, rows*stride);
    }   

    T& operator[](int i) const noexcept {
        return this->operator()(i);
    }

    T& operator()(int row, int col) const noexcept {
        // assert(row < rows && col < cols);
        return *(T*)(((char*)data)+row*stride+col*sizeof(T));
    }

    T& operator()(int i) const noexcept {
        return this->operator()(i/row, i%row);
    }

    template<typename O = T>
    O* ptr() const noexcept {
        return (O*)data;
    }

    CvMat operator()(const CvRect &rect) const {
        char *ptr = (char *)data;
        CvMat block((T*)(ptr + rect.y*stride+rect.x*sizeof(T)), rect.height, rect.width, stride);
        block.mem_ptr = this->mem_ptr;
        block.ref_cnt = this->ref_cnt;
        if (block.ref_cnt) {
            block.ref_cnt[0]++;
        }
        return block;
    }

    int colStride() const noexcept {
        return stride;
    }
    
    void resize(int rows_, int cols_, int stride_ = -1) {
        if (data && rows_*cols_ <= rows*cols) {
            reshape(rows_, cols_);
        } else {
            if (stride_ == -1) {
                stride_ = cols_*sizeof(T);
            }
            void * prev = mem_ptr;
            mem_ptr = memalign(alginN, ((rows_*stride_+3)&(~0x03))+sizeof(int)); 
            assert(mem_ptr != NULL);
            if (prev) {
                free(prev);
            }
            rows = rows_;
            cols = cols_;
            stride = stride_;
            data = (T*)mem_ptr;
            ref_cnt = (int*)(((char*)data) + ((rows_*stride_+3)&(~0x03)));
            ref_cnt[0] = 1;
        }
    }

    void reshape(int rows_, int cols_) {
        assert(rows_*cols_ <= cols*rows);
        rows = rows_;
        cols = cols_;
        stride = cols_*sizeof(T);
    }

    T *data;
    int rows;
    int cols;
    int stride; // col stride in bytes
private:
    void *mem_ptr;
    int *ref_cnt;
};

template<typename T>
T deg2rad(T deg) {
    return deg * static_cast<T>(M_PI/180.0);
}

template<typename T>
T rad2deg(T deg) {
    return deg * static_cast<T>(180.0/M_PI);
}

typedef CvMat<uint8_t> CvMatU8;
typedef CvMat<int8_t> CvMatInt8;
typedef CvMat<uint16_t> CvMatU16;
typedef CvMat<int16_t> CvMatInt16;
typedef CvMat<uint32_t> CvMatU32;
typedef CvMat<int32_t> CvMatInt32;
typedef CvMat<int64_t> CvMatInt64;

typedef CvMat<float> CvMatF32;
typedef CvMat<float complex> CvMatComplex;

typedef CvMat<hfq15_t> CvMat_hfq15;
typedef CvMat<wfq15_t> CvMat_wfq15;
typedef CvMat<wfq16_t> CvMat_wfq16;
typedef CvMat<cwfq16_t> CvMat_cwfq16;
typedef CvMat<cwfq8_t> CvMat_cwfq8;
typedef CvMat<cwfq7_t> CvMat_cwfq7;
typedef CvMat<chfq8_t> CvMat_chfq8;

// void splitCvMatF32Complex(CvMatF32Complex &a, CvMatF32 &real, CvMatF32 &imag)
// {
//     real.resize(a.rows, a.cols);
//     imag.resize(a.rows, a.cols);
//     for (int r = 0; r < a.rows; r++) {
//         for (int c = 0; c < a.cols; c++) {
//             real(r, c) = qhcomplex_creal_f(a(r, c));
//             imag(r, c) = qhcomplex_cimag_f(a(r, c));
//         }
//     }
// }

#if 0

#define PRINT_CVMATU8(label, m, x, y, w, h)    if (gDbg){ \
        printf("--------watch-----%s:[%d,%d,%d,%d]--%p--\n", label, x, y, w, h, m.data); \
        char buff[2048]={0}; \
        int offset = 0; \
        for (int i = y; i < y+h; i++) { \
            offset = 0; \
            for (int j = x; j < x+w; j++) { \
                offset += sprintf(buff+offset, "%3d, ", m(i,j)); \
            } \
            printf("%s\n", buff); \
        } \
    }
#define PRINT_CVMATfqN(label, m, x, y, w, h, n)  if (gDbg){ \
        printf("--------watch-----%s:[%d,%d,%d,%d]--%p--\n", label, x, y, w, h, m.data); \
        char buff[2048]={0}; \
        int offset = 0; \
        for (int i = y; i < y+h; i++) { \
            offset = 0; \
            for (int j = x; j < x+w; j++) { \
                offset += sprintf(buff+offset, "%.4f, ", ((float)m(i,j))/(1<<n)); \
            } \
            printf("%s\n", buff); \
        } \
    }
#define PRINT_CVMATfuq7(label, m, x, y, w, h) PRINT_CVMATfqN(label, m, x, y, w, h, 8)

#define PRINT_CVMATfq6(label, m, x, y, w, h) PRINT_CVMATfqN(label, m, x, y, w, h, 6)
#define PRINT_CVMATfq7(label, m, x, y, w, h)  PRINT_CVMATfqN(label, m, x, y, w, h, 7) 
#define PRINT_CVMATfq8(label, m, x, y, w, h) PRINT_CVMATfqN(label, m, x, y, w, h, 8)
#define PRINT_CVMATfq15(label, m, x, y, w, h) PRINT_CVMATfqN(label, m, x, y, w, h, 15)
#define PRINT_CVMATfq16(label, m, x, y, w, h) PRINT_CVMATfqN(label, m, x, y, w, h, 16)

#define PRINT_CVMATF32(label, m, x, y, w, h)    if (gDbg){ \
        printf("--------watch-----%s:[%d,%d,%d,%d]--%p--\n", label, x, y, w, h, m.data); \
        char buff[2048]={0}; \
        int offset = 0; \
        for (int i = y; i < y+h; i++) { \
            offset = 0; \
            for (int j = x; j < x+w; j++) { \
                offset += sprintf(buff+offset, "%.4f, ", m(i,j)); \
            } \
            printf("%s\n", buff); \
        } \
    }

#define PRINT_CVMATcfqN(label, m, x, y, w, h, fun_c2f)   if (gDbg){ \
        printf("--------watch-----%s:[%d,%d,%d,%d]--%p--\n", label, x, y, w, h, m.data); \
        char buff[2048]={0}; \
        int offset = 0; \
        for (int i = y; i < y+h; i++) { \
            offset = 0; \
            for (int j = x; j < x+w; j++) { \
                float re, im;  \
                fun_c2f(m(i, j), re, im); \
                offset += sprintf(buff+offset, "%.4f+%.4fi, ", re, im); \
            } \
            printf("%s\n", buff); \
        } \
    }

#define PRINT_CVMATcwfq7(label, m, x, y, w, h) PRINT_CVMATcfqN(label, m, x, y, w, h, cwfq7_to_float) 
#define PRINT_CVMATcwfq8(label, m, x, y, w, h) PRINT_CVMATcfqN(label, m, x, y, w, h, cwfq8_to_float) 
#define PRINT_CVMATcwfq11(label, m, x, y, w, h) PRINT_CVMATcfqN(label, m, x, y, w, h, cwfq11_to_float)
#define PRINT_CVMATcwfq16(label, m, x, y, w, h) PRINT_CVMATcfqN(label, m, x, y, w, h, cwfq16_to_float)
#define PRINT_CVMATcwfq18(label, m, x, y, w, h) PRINT_CVMATcfqN(label, m, x, y, w, h, cwfq18_to_float)
#define PRINT_CVMATchfq15(label, m, x, y, w, h) PRINT_CVMATcfqN(label, m, x, y, w, h, chfq15_to_float)
#define PRINT_CVMATchfq8(label, m, x, y, w, h)  PRINT_CVMATcfqN(label, m, x, y, w, h, chfq8_to_float)
#define PRINT_CVMATComplex(label, m, x, y, w, h)    if (gDbg){ \
        printf("--------watch-----%s:[%d,%d,%d,%d]--%p--\n", label, x, y, w, h, m.data); \
        char buff[2048]={0}; \
        int offset = 0; \
        for (int i = y; i < y+h; i++) { \
            offset = 0; \
            for (int j = x; j < x+w; j++) { \
                offset += sprintf(buff+offset, "%.4f+%.4fi, ", qhcomplex_creal_f(m(i,j)), qhcomplex_cimag_f(m(i,j))); \
            } \
            printf("%s\n", buff); \
        } \
    }

#else
#define PRINT_CVMATF32(label, m, x, y, w, h)
#define PRINT_CVMATComplex(label, m, x, y, w, h)
#define PRINT_CVMATfq6(label, m, x, y, w, h)
#define PRINT_CVMATfq7(label, m, x, y, w, h)
#define PRINT_CVMATcwfq7(label, m, x, y, w, h)
#define PRINT_CVMATcwfq8(label, m, x, y, w, h)
#define PRINT_CVMATcwfq11(label, m, x, y, w, h)
#define PRINT_CVMATcwfq18(label, m, x, y, w, h)
#define PRINT_CVMATfuq7(label, m, x, y, w, h) 
#define PRINT_CVMATfq8(label, m, x, y, w, h) 
#define PRINT_CVMATfq15(label, m, x, y, w, h) 
#define PRINT_CVMATfq16(label, m, x, y, w, h) 
#define PRINT_CVMATcwfq16(label, m, x, y, w, h)
#define PRINT_CVMATchfq15(label, m, x, y, w, h) 
#define PRINT_CVMATchfq8(label, m, x, y, w, h) 
#endif

#endif // CV_MAT_HPP
